#include "navigation_task.h"
#include "battery_level_check.h"
#include "screen_return_zero_task.h"
#include "screen_return_zero_check.h"
#include "docking_task.h"
#include "task/task_controller.h"
#include "scheduler/sequence_scheduler.h"
#include "scheduler/fallback_scheduler.h"
#include "switch_high_current.h"
#include "scheduler/scheduler_factory.h"

int main()
{
    task::TaskController task_controller;
    task_controller.setMaxParallelTaskCount(4);//最多同时运行4个任务

    auto battery_level_check = task::TaskFactory<BatteryLevelCheck>::createTask("battery_level_check");
    auto screen_return_zero_check = task::TaskFactory<ScreenReturnZeroCheck>::createTask("screen_return_zero_check");
    auto screen_return_zero_task = task::TaskFactory<ScreenReturnZeroTask>::createTask("screen_return_zero_task");
    auto navigation_task = task::TaskFactory<NavigationTask>::createTask("navigation_task");
    auto docking_task = task::TaskFactory<DockingTask>::createTask("docking_task");
    auto switch_high_current = task::TaskFactory<SwitchHighCurrent>::createTask("switch_high_current");

    auto screen_return_scheduler = scheduler::SchedulerFactory<scheduler::FallbackScheduler>::createScheduler("screen_return_scheduler");
    screen_return_scheduler->appendTask(screen_return_zero_check);
    screen_return_scheduler->appendTask(screen_return_zero_task);

    auto recharge_scheduler = scheduler::SchedulerFactory<scheduler::SequenceScheduler>::createScheduler("recharge_task");
    recharge_scheduler->appendScheduler(screen_return_scheduler);
    recharge_scheduler->appendTask(navigation_task);
    recharge_scheduler->appendTask(docking_task);
    recharge_scheduler->appendTask(switch_high_current);

    auto recharge_task_scheduler = scheduler::SchedulerFactory<scheduler::SequenceScheduler>::createScheduler("recharge_task_scheduler");
    recharge_task_scheduler->appendTask(battery_level_check);
    recharge_task_scheduler->appendScheduler(recharge_scheduler);

    auto task_chain = new task::TaskChain("recharge", recharge_task_scheduler);

    RobotPose robot_pose = RobotPose();
    robot_pose.setX(1);
    robot_pose.setY(2);
    robot_pose.setYaw(3);
    task_chain->setData<RobotPose>("robot_pose", robot_pose);

    task_controller.startTaskChain(task_chain);

    while (true)
    {

    }
}
